#include "backposition.h"

string Backposition:: command(Parser parser) 
{

    double power=50;
    float  dir_target=0;
    float  dist_target=0;

    bool veo_flag=false;
    vector <float> aux(2);
    parser.getInitPos(aux,id,path);

/*** Asignamos el flag al que deben ir en funcion de su dorsal (posicion) ***/
    switch (id)
       {
       case 2://lat derecho
          for (int i=0; i<3; i++)
	  {  
             if(parser.field=="right"){
                dist_target=parser.flag_p_r_t[0];
                dir_target=parser.flag_p_r_t[1];
             }
             else{
                dist_target=parser.flag_p_l_b[0];
                dir_target=parser.flag_p_l_b[1];
             }
	  }
          break;
       case 3://lat izquierdo
          for (int i=0; i<3; i++)
	  {
             if(parser.field=="right"){
                dist_target=parser.flag_p_r_b[0];
                dir_target=parser.flag_p_r_b[1];
             }
             else{
                dist_target=parser.flag_p_l_t[0];
                dir_target=parser.flag_p_l_t[1];
             }
	  }
          break;
       case 4://central dcho
          for (int i=0; i<3; i++)
	  {
             if(parser.field=="right"){
                dist_target=parser.flag_p_r_c[0];
                dir_target=parser.flag_p_r_c[1];
             }
             else{
                dist_target=parser.flag_p_l_c[0];
                dir_target=parser.flag_p_l_c[1];
             }
	  }
          break;
       case 5://central izq
          for (int i=0; i<3; i++)
	  {
             if(parser.field=="right"){
                dist_target=parser.flag_p_r_c[0];
                dir_target=parser.flag_p_r_c[1];
             }
             else{
                dist_target=parser.flag_p_l_c[0];
                dir_target=parser.flag_p_l_c[1];
             }
	  }
          break;
       case 6://medio centro
          for (int i=0; i<3; i++)
	  {
             if(parser.field=="right"){
                dist_target=parser.flag_p_r_c[0];
                dir_target=parser.flag_p_r_c[1];
             }
             else{
                dist_target=parser.flag_p_l_c[0];
                dir_target=parser.flag_p_l_c[1];
             }
	  }
          break;
       case 7://medio dcha
          for (int i=0; i<3; i++)
	  {
             if(parser.field=="right"){
                dist_target=parser.flag_p_r_t[0];
                dir_target=parser.flag_p_r_t[1];
             }
             else{
                dist_target=parser.flag_p_l_b[0];
                dir_target=parser.flag_p_l_b[1];
             }
	  }
          break;
       case 8://medio izq
          for (int i=0; i<3; i++)
	  {
             if(parser.field=="right"){
                dist_target=parser.flag_p_r_b[0];
                dir_target=parser.flag_p_r_b[1];
             }
             else{
                dist_target=parser.flag_p_l_t[0];
                dir_target=parser.flag_p_l_t[1];
             }
	  }
          break;
       case 9://delantero centro
          for (int i=0; i<3; i++)
	  {
             //dist_target=parser.flag_c[0];
             //dir_target=parser.flag_c[1];
             if(parser.field=="right"){
                dist_target=parser.flag_p_l_c[0];
                dir_target=parser.flag_p_l_c[1];
             }
             else{
                dist_target=parser.flag_p_r_c[0];
                dir_target=parser.flag_p_r_c[1];
             }
	  }
          break;
       case 10://extremo dcho
          for (int i=0; i<3; i++)
	  {
             /*if(parser.field=="right"){
                //dist_target=parser.flag_c_t[0];
                //dir_target=parser.flag_c_t[1];
             }
             else{
                //dist_target=parser.flag_c_b[0];
                //dir_target=parser.flag_c_b[1];
             }*/
             dist_target=parser.flag_c[0];
             dir_target=parser.flag_c[1];
	  }
          break;
       case 11://extremo izq
          for (int i=0; i<3; i++)
	  {
             if(parser.field=="right"){
                //dist_target=parser.flag_c_b[0];
                //dir_target=parser.flag_c_b[1];
                dist_target=parser.flag_p_l_c[0];
                dir_target=parser.flag_p_l_c[1];
             }
             else{
                //dist_target=parser.flag_c_t[0];
                //dir_target=parser.flag_c_t[1];
                dist_target=parser.flag_p_r_c[0];
                dir_target=parser.flag_p_r_c[1];
             }
	  }
          break;
        }

/*** Buscamos el flag ***/
    if((dist_target == 0) && (dir_target == 0)) //**no veo el flag
    {
        posicion=false;
        veo_flag = false;
        if ((aux[1]>=0)){
	  if(parser.field=="left")
            output<<turn(-moment);
	  else if(parser.field=="right")
            output<<turn(moment);
	}
        else if((aux[1]<0)){

	  if(parser.field=="left")
            output<<turn(moment);
	  else if(parser.field=="right")
            output<<turn(-moment);
        
	}
    }

    else if((dir_target > -15) && (dir_target < 15)) //**veo el flag
    {
        //cout << "Veo el objetivo " << dir_target << " grados." << endl;
        veo_flag = true;
    }
    else //**giro para buscarlo
    {
        posicion=false;
        veo_flag=false;
	moment+=dir_target*0.1; 
	if (moment<-15 )
		moment=-15; 
	if (moment>15)
		moment=15; 

        //cout << "Giro y veo la el objetivo a " << dir_target << " grados." << endl;
        if(dir_target>5)
            output<<turn(moment);

        else if(dir_target<-5)
            output<<turn(-moment);
    }
/*** Voy a al objetivo ***/
    if (veo_flag)
    {
       switch (id)
       {
       case 2: case 3: case 4: case 5:
          if(dist_target > 7)
          {
             power = 70;
             output<<run(power);
             posicion=false;
             //cout<<"Voy a mi posicion"<<endl;
          }
          else{
             posicion = true;
             output<<turn(180);
             //cout<<"En mi posicion" << dist_target <<endl;
          }
          break;
       case 6: 
          if(dist_target > 15)
          {
             power = 70;
             output<<run(power);
             posicion=false;
             //cout<<"Voy a mi posicion"<<endl;
          }
          else{
             posicion = true;
             output<<turn(180);
             cout<<"En mi posicion " << dist_target <<endl;
          }
          break;
       case 7: case 8: 
          if(dist_target > 17)
          {
             power = 70;
             output<<run(power);
             posicion=false;
             //cout<<"Voy a mi posicion"<<endl;
          }
          else{
             posicion = true;
             output<<turn(180);
             //cout<<"En mi posicion" << dist_target <<endl;
          }
          break;
       case 9: case 11:
          if(dist_target > 22)
          {
             power = 40;
             output<<run(power);
             posicion=false;
             //cout<<"Voy a mi posicion"<<endl;
          }
          else{
             posicion = true;
             output<<turn(0);
             //cout<<"En mi posicion" << dist_target <<endl;
          }
          break;
       case 10: 
          if(dist_target > 10)
          {
             power = 40;
             output<<run(power);
             posicion=false;
             //cout<<"Voy a mi posicion"<<endl;
          }
          else{
             posicion = true;
             output<<turn(0);
             //cout<<"En mi posicion" << dist_target <<endl;
          }
          break;
       }
    }
    parser.init(); 

    return output.str();
}



